//Created 1/20/2015 by Christopher Allum with Shehzeen Hussain.
/*
This class reads an arduino analog pin and extracts basic information.
So far features extracted are:
        - mean
        -add comments
//hihihighghgh
CHANGE LOGS
1/20/2015    Christopher Allum & Shehzeen Hussain  
        Created
1/24/2015    Christopher Allum   
        Major refactors, Discovered major array out of bounds error causing silent crash.
        Completed the mean feature extraction, Added offset input from 0 to 5 scale
1/26/2015    Christopher Allum & Shehzeen Hussain
        Added a frequency analysis option and fixed the two minor bugs not worthy of note.

1/28/2015    Christopher Allum
        Integrated the EMG.h class into main E-mu code and refactored input 
        parameters to become: readEMG(double* emgData);
2/16/2015    Christopher Allum
        Integrated the FFT code into this class. Added a few functions. 
        Changed a little in the main too.
2/19/2015    Christopher Allum
        Created Two Instances of fft processing. temporarily removed averaging to see if it
        affects IMU data which also uses analog pins. Just duplicated all FFT variables and functions
        because current implementation used a lot of globals... I really want to redo this later because
        it's terribly sloppy! >:(
*/

#define emgWindowSize 10 //THE CIRCULAR BUFFER OVER WHICH FEATURES ARE EXTRACTED



#define rmsFFT_Size 70

double offsetVoltageOne = 2.2;
double offsetVoltageTwo = 2.2;
int offsetAmtOne = (offsetVoltageOne * 1024 / 5);//converts from a 0to5 scale to a 0 to 1023 scale
int offsetAmtTwo = (offsetVoltageOne * 1024 / 5);//converts from a 0to5 scale to a 0 to 1023 scale

#define ARM_MATH_CM4
#include <arm_math.h>

int SAMPLE_RATE_HZ = 2000;             // Sample rate of the audio in hertz.
const int FFT_SIZE = 256;              // Size of the FFT. Realistically can only be at most 256 
                                       // without running out of memory for buffers and other state.
const int ANALOG_READ_RESOLUTION = 10; // Bits of resolution for the ADC.
const int ANALOG_READ_AVERAGING = 4;  // Number of samples to average with each ADC reading.

IntervalTimer samplingTimerOne;
IntervalTimer samplingTimerTwo;

float samplesOne[FFT_SIZE*2];
float magnitudesOne[FFT_SIZE];
int sampleCounterOne = 0;

float samplesTwo[FFT_SIZE*2];
float magnitudesTwo[FFT_SIZE];
int sampleCounterTwo = 0;



//RUNTIME OPTIONS
//debug options
//boolean debugEmgBuffer = false;
//boolean debugFeatureExtraction = false;

//output switches
//boolean displayOutput = false;
//boolean printFFT = false;
//VARIABLES
int inputOne = A1;//Later there will be two pins, A1, and A2
int inputTwo = A2;

int emgWindowIterOne = 0;
int emgWindowIterTwo = 0;
float emgValueOne[emgWindowSize];
float emgValueTwo[emgWindowSize];


//FEATURES TO EXTRACT
double timeMeanOne;
double rmsOne;
int numCrossOne;

double timeMeanTwo;
double rmsTwo;
int numCrossTwo;



//====FREQUENCY DOMAIN FEATURE EXTRACTION METHODS==============
void samplingCallbackOne() {
      //Serial.println("ola");
      // Read from the ADC and store the sample data  
      samplesOne[sampleCounterOne] = (float32_t)analogRead(inputOne);
      // Complex FFT functions require a coefficient for the imaginary part of the input.
      // Since we only have real data, set this coefficient to zero.
      samplesOne[sampleCounterOne+1] = 0.0;
      // Update sample buffer position and stop after the buffer is filled
      sampleCounterOne += 2;   
      if (sampleCounterOne >= FFT_SIZE*2) {    
        samplingTimerOne.end();
  }
}
void samplingCallbackTwo() {
      // Read from the ADC and store the sample data  
      samplesTwo[sampleCounterTwo] = (float32_t)analogRead(inputTwo);
      // Complex FFT functions require a coefficient for the imaginary part of the input.
      // Since we only have real data, set this coefficient to zero.
      samplesTwo[sampleCounterTwo+1] = 0.0;
      // Update sample buffer position and stop after the buffer is filled
      sampleCounterTwo += 2;   
      if (sampleCounterTwo >= FFT_SIZE*2) {    
        samplingTimerTwo.end();
  }
}
void samplingBeginOne() {
  // Reset sample buffer position and start callback at necessary rate.
  //Serial.println("sampling1 begin");
  sampleCounterOne = 0;
  samplingTimerOne.begin(samplingCallbackOne, 1000000/SAMPLE_RATE_HZ);
}
void samplingBeginTwo() {
  // Reset sample buffer position and start callback at necessary rate.
  sampleCounterTwo = 0;
  samplingTimerTwo.begin(samplingCallbackTwo, 1000000/SAMPLE_RATE_HZ);
  
}

boolean samplingIsDoneOne() {
  return sampleCounterOne >= FFT_SIZE*2;
}
boolean samplingIsDoneTwo() {
  return sampleCounterTwo >= FFT_SIZE*2;
}

double getFreqMean(float* magnitudes){
  double sum = 0;
  for(int x=0; x < FFT_SIZE; x++)  {
    sum += magnitudes[x];
  }
  return (sum / FFT_SIZE);
}

int findPeak(float* magnitude){
  float peakFrequency = 0;
  int binNum = 0;
  for (int i = 1; i < FFT_SIZE/2; ++i) {
    if (magnitude[i] > peakFrequency){
        peakFrequency = magnitude[i];
        binNum = i; //store the bin number of the peak freq
    } 
  }
     
     return (int(binNum*SAMPLE_RATE_HZ/FFT_SIZE));
}
//====TIME DOMAIN FEATURE EXTRACTION METHODS==============
double getTimeMean(float* emgValue){
  double sum = 0;
  for(int x=0; x < emgWindowSize; x++)  {
    sum += emgValue[x];
  }
  return (sum / emgWindowSize);
}
/*
double getRMS(float* emgValue){ // the power of te signal
  //Serial.println("getting rMS");
  double total = 0;
  for(int r = 0; r <emgWindowSize; r++){
    total += emgValue[r] * emgValue[r];  
  }
  total = total/emgWindowSize;
  return sqrt(total);
}*/
double getFFT_RMS(float* emgValue){ // the power of te signal
  //Serial.println("getting rMS");
  double total = 0;
  for(int r = 1; r < rmsFFT_Size; r++){
    total += emgValue[r] * emgValue[r];  
  }
  total = total/rmsFFT_Size;
  return sqrt(total);
}

int getZeroCross(float* emgValue){
  int counter = 0;
  for(int r = 0; r < emgWindowSize; r++){
    if ((emgValue[r] < 0 && emgValue[r-1] > 0) || (emgValue[r-1] < 0 && emgValue[r] > 0)){
      counter++;
    }
  }
  return counter;
}
/*
void printEMGWindow(){
  //PRINT WINDOW  
  for(int x = 0; x < emgWindowSize; x++){
    if(x != emgWindowIter){ Serial.println(emgValue[x]);}
    else{
      Serial.print(emgValue[x]);
      Serial.println(" * ");
    }
  }
}
*/
/*
void printFeatures(){
  Serial.println("RESULTS");
  Serial.print("RMS: ");
  Serial.print(rms);
  Serial.print("\t");
  Serial.print("mean: ");
  Serial.print(timeMean);
  Serial.print("\t");
  Serial.print("numCross: ");
  Serial.print(numCross);
  Serial.print("\t");

  Serial.println();
}
*/
void subtractOffsetOne(){
  emgValueOne[emgWindowIterOne] -= offsetAmtOne; //subtract offset
}
void subtractOffsetTwo(){
  emgValueTwo[emgWindowIterTwo] -= offsetAmtTwo; //subtract offset
}

void writeEMG_ToSens(float* emgData){
  emgData[6] = emgValueOne[emgWindowIterOne];
  emgData[7] = emgValueTwo[emgWindowIterTwo];
}

//   0    1        2      3    4    5      6        7        8        9        10          
//[jerkX, jerkY, jerkZ, rotX, rotY, rotZ, emgAmp1, emgAmp2, emg1RMS, emg2RMS, emg1PeakFreq, 
//  11           12            13             14              15            16              17
//emg2PeakFreq, pattern1Score, pattern2Score, pattern3Score, pattern4Score, pattern5Score, pattern6Score
//FFT data 1: 20-99
//FFT data 2: 100-179
void writeEMG_FFT_ToData(float* data){
 
  //write fft data
  if(samplingIsDoneOne()){
    //Serial.println("fft1 ready");
    for(int i = 0; i < 80; i++){
      data[i+20] = magnitudesOne[i+1];
    }
  }
  if(samplingIsDoneTwo()){
    //Serial.println("fft2 ready");
    for(int i = 0; i < 80; i++){
      data[i+100] = magnitudesTwo[i+1];
    }
  }
}

void writeEMG_ToData(float* data, float ampSens, float rmsSens, float modSens){
  data[6] = emgValueOne[emgWindowIterOne]*ampSens;
  data[7] = emgValueTwo[emgWindowIterTwo]*ampSens;
  data[8] = rmsOne*rmsSens;
  data[9] = rmsTwo*rmsSens;
  data[10] = findPeak(magnitudesOne)*modSens;
  data[11] = findPeak(magnitudesTwo)*modSens;
}

void initializeEMG(){
   // Set up ADC and audio input.
  pinMode(inputOne, INPUT);
  pinMode(inputTwo, INPUT);
  samplingBeginOne();
  samplingBeginTwo();
  //analogReadResolution(ANALOG_READ_RESOLUTION);
  //analogReadAveraging(ANALOG_READ_AVERAGING); 
}


void readEMG(float* data, float ampSens, float rmsSens, float modSens){
   
 
  // Calculate FFT if a full sample is available.
  //Serial.println("reading emg");
  //Serial.println(sampleCounterOne);
  if (samplingIsDoneOne()) {
    // Run FFT on sample data. 
    //Serial.println("data for first fft is ready!!!");
    arm_cfft_radix4_instance_f32 fft_inst;
    arm_cfft_radix4_init_f32(&fft_inst, FFT_SIZE, 0, 1);
    arm_cfft_radix4_f32(&fft_inst, samplesOne);
    // Calculate magnitude of complex numbers output by the FFT.
    arm_cmplx_mag_f32(samplesOne, magnitudesOne, FFT_SIZE);
    
    writeEMG_FFT_ToData(data);
    samplingBeginOne();
  }
    if (samplingIsDoneTwo()) {
    // Run FFT on sample data. 
    arm_cfft_radix4_instance_f32 fft_instTwo;
    arm_cfft_radix4_init_f32(&fft_instTwo, FFT_SIZE, 0, 1);
    arm_cfft_radix4_f32(&fft_instTwo, samplesTwo);
    // Calculate magnitude of complex numbers output by the FFT.
    arm_cmplx_mag_f32(samplesTwo, magnitudesTwo, FFT_SIZE);
    writeEMG_FFT_ToData(data);
    samplingBeginTwo();
  }
  //read the analog pin. 
  emgValueOne[emgWindowIterOne] = analogRead(inputOne);
  emgValueTwo[emgWindowIterTwo] = analogRead(inputTwo);
  
  subtractOffsetOne();//subtracts offset and makes all values positive
  subtractOffsetTwo();//subtracts offset and makes all values positive
 
  //EXTRACT FEATURES
  //numCrossOne = getZeroCross(emgValueOne);
  //numCrossTwo = getZeroCross(emgValueTwo);
  //rmsOne = getRMS(emgValueOne);//could be used, but FFT RMS may be more valuable
  //rmsTwo = getRMS(emgValueTwo);
  rmsOne = getFFT_RMS(magnitudesOne);
  rmsTwo = getFFT_RMS(magnitudesTwo);
  //timeMeanOne = getTimeMean(magnitudesOne);
  //timeMeanTwo = getTimeMean(magnitudesTwo);
  writeEMG_ToData(data, ampSens, rmsSens, modSens);
  //PRINT RESULTS
  /*if(displayOutput){
    printEMGWindow();
    printFeatures(); 
  }
   //debug info for circular buffer
  if(debugEmgBuffer){
    Serial.print("DEBUG INFO ---- ");
    Serial.print("emgWindowIter: ");
    Serial.println(emgWindowIter);
  }
  */
  //ITERATE THROUgh BUFFER ONCE
  if(emgWindowIterOne < emgWindowSize){
    emgWindowIterOne++;
  }
  if(emgWindowIterTwo < emgWindowSize){
    emgWindowIterTwo++;
  }
  
  if(emgWindowIterOne >= emgWindowSize){ //If you're at the end of the buffer then reset the iterator
    /*if(debugEmgBuffer){
      Serial.print("DEBUG INFO ---- ");
      Serial.println("resetting emgWindowIter to 0 now");
    }*/
    emgWindowIterOne = 0;
  }
    if(emgWindowIterTwo >= emgWindowSize){ //If you're at the end of the buffer then reset the iterator
    emgWindowIterTwo = 0;
  }
}



